#include <ros/ros.h>

#include <atomic>

#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<std_msgs/UInt64.h>
#include<sensor_msgs/PointCloud2.h>

#include "unionsys_core.hpp"

#include"../SDPltUtils/SDPltBase.hpp"
#include "../SDPltDDS/SDPltPublisher.hpp"
#include "../SDPltDDS/SDPltSubscriber.hpp"

#include <fastdds/dds/publisher/Publisher.hpp>

#include "../SDPltDDSPose/SDPltPoseMsg.h"
#include "../SDPltDDSPose/SDPltPoseMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltVelocityMsg.h"
#include "../SDPltDDSPose/SDPltVelocityMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltStateMsg.h"
#include "../SDPltDDSPose/SDPltStateMsgPubSubTypes.h"
#include "../SDPltDDSPose/pointcloud2.h"
#include "../SDPltDDSPose/pointcloud2PubSubTypes.h"

#include<std_msgs/Int32MultiArray.h>

//接收ros发布的topic,后dds再发布topic


//声明fastdds发布、ros的订阅类
SDPltPublisher<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddspub_goal=nullptr;



RegisterMSN *reg_pub = new RegisterMSN();


int drone_state = 0;
int self_drone_id;

int drone_select_arr[5] = {0,0,0,0,0};

ros::Subscriber drone_select_sub, goal_sub;



void drone_select_cb(const std_msgs::Int32MultiArray::ConstPtr &msg)
{
    for(int i=0;i<5;i++)
    {
        drone_select_arr[i] = msg->data[i];
    }
}

void goal_cb(const geometry_msgs::PoseStamped::ConstPtr &goal_msg)
{
    SDPltPoseMsg *PoseDDSMsg = new SDPltPoseMsg();

    //赋值
    PoseDDSMsg->seq_(goal_msg->header.seq);
    PoseDDSMsg->frame_id_(goal_msg->header.frame_id);
    PoseDDSMsg->xPoint_(goal_msg->pose.position.x);
    PoseDDSMsg->yPoint_(goal_msg->pose.position.y);
    PoseDDSMsg->zPoint_(goal_msg->pose.position.z);
    PoseDDSMsg->xOrientation_(goal_msg->pose.orientation.x);
    PoseDDSMsg->yOrientation_(goal_msg->pose.orientation.y);
    PoseDDSMsg->zOrientation_(goal_msg->pose.orientation.z);
    PoseDDSMsg->wOrientation_(goal_msg->pose.orientation.w);
    ddspub_goal->publish(*PoseDDSMsg);
    delete PoseDDSMsg;
}


int main(int argc, char* argv[]){
    ros::init(argc, argv, "SDPltDDSPosepub");
    ros::NodeHandle nodeHandle; 
    reg_pub->detect_serial_number();

    // 设置循环的频率
    ros::Rate loop_rate(100);



    //先接收ros发布的topic,后dds再发布topic
    goal_sub = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/zhao/goal", 10, goal_cb);
    ddspub_goal = new SDPltPublisher<SDPltPoseMsgPubSubType, SDPltPoseMsg>("ddsGoaltopic","SDPltPoseMsg",BEST_EFFORT_RELIABILITY_QOS);
    
    // drone_select_sub = nodeHandle.subscribe<std_msgs::Int32MultiArray>("/rviz/drone_selection", 10,drone_select_cb);



    //循环回调
    while(ros::ok()&&nodeHandle.ok()){        
        ros::spinOnce();
        loop_rate.sleep();
    }
    // ros::spin();


    nodeHandle.shutdown();
    return 0;
}
